﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace read2imu
{
    internal class data
    {
        public enum imu_type
        {
            HWT9052_485 = 1,
            WT901C_485,
        }

        public enum laser
        {
            laser1 = 0x03,
            laser2,
            laser3
        }

        /// <summary>
        /// CRC校验，参数data为byte数组，data_length
        /// </summary>
        /// <param name="data">校验数据，字节数组</param>
        /// <returns>字节0是高8位，字节1是低8位</returns>
        public static byte[] CRC16(byte[] data, int data_length)
        {
            //crc计算赋初始值
            int crc = 0xffff;
            for (int i = 0; i < data_length; i++)
            {
                crc = crc ^ data[i];
                for (int j = 0; j < 8; j++)
                {
                    int temp;
                    temp = crc & 1;
                    crc = crc >> 1;
                    crc = crc & 0x7fff;
                    if (temp == 1)
                    {
                        crc = crc ^ 0xa001;
                    }
                    crc = crc & 0xffff;
                }
            }
            //CRC寄存器的高低位进行互换
            byte[] crc16 = new byte[2];
            //CRC寄存器的高8位变成低8位，
            crc16[1] = (byte)((crc >> 8) & 0xff);
            //CRC寄存器的低8位变成高8位
            crc16[0] = (byte)(crc & 0xff);
            return crc16;
        }

        public static void save_data_dialog(RichTextBox rtb)
        {
            SaveFileDialog saveDataDialog = new SaveFileDialog();
            saveDataDialog.InitialDirectory = System.Windows.Forms.Application.StartupPath;
            string yyyyMMddhh = DateTime.Now.ToString("yyyy-MM-dd-HH-mm-ss");
            saveDataDialog.FileName = yyyyMMddhh;
            saveDataDialog.Title = "保存数据文件";
            saveDataDialog.AddExtension = true;
            saveDataDialog.Filter = "RTF文件(*.rtf)|*.rtf";
            if (saveDataDialog.ShowDialog() == DialogResult.OK)
            {
                rtb.SaveFile(saveDataDialog.FileName);
                MessageBox.Show("导出成功，文件路径为：" + saveDataDialog.FileName);
            }
            else
            { }
        }

        public static void save_data_dialog()
        {
            SaveFileDialog saveDataDialog = new SaveFileDialog();
            saveDataDialog.InitialDirectory = System.Windows.Forms.Application.StartupPath;
            string yyyyMMddhh = DateTime.Now.ToString("yyyy-MM-dd-HH-mm-ss");
            saveDataDialog.FileName = yyyyMMddhh;
            saveDataDialog.Title = "保存数据文件";
            saveDataDialog.AddExtension = true;
            saveDataDialog.Filter = "Excel (*.xlsx)|*.xlsx";
            //openImageDialog.Multiselect = false;
            if (saveDataDialog.ShowDialog() == DialogResult.OK)
            {
                if (saveDataDialog.FileName.EndsWith(".xlsx"))
                    new Excell().WriteExcel_Table(saveDataDialog.FileName, mainForm.dataGridView_data);
                //else if (saveDataDialog.FileName.EndsWith(".txt") || saveDataDialog.FileName.EndsWith(".csv"))
                //    ExportToExcelOrTxt(dataGridView1, saveDataDialog.FileName);
                MessageBox.Show("导出成功，文件路径为：" + saveDataDialog.FileName);
            }
            else
            { }
        }

        public static byte[] read_cmd_generate(byte ID, byte start_byte, int num)
        {
            byte[] b = new byte[8];
            b[0] = ID;
            b[1] = 0x03;
            b[2] = 0;
            b[3] = start_byte;
            b[4] = 0;
            b[5] = Convert.ToByte(num);
            byte[] CRC_byte = CRC16(b, 6);
            b[6] = CRC_byte[0];
            b[7] = CRC_byte[1];
            return b;
        }

        public static void data_decode(int receive_length, byte[] receive_data)
        {
            if (receive_length == 0) return;
            if (receive_data[0] == (byte)imu_type.HWT9052_485 && receive_data[1] == 0x03)
            {
                if (receive_length == 17)
                {
                    byte[] CRC_byte = CRC16(receive_data, 15);
                    if (CRC_byte[0] != receive_data[15] || CRC_byte[1] != receive_data[16]) return;
                    for (int k = 0; k < 3; k++)
                    {
                        mainForm.angle1[k] = angle4decode(receive_data, 3 + 4 * k);
                        //Console.Write(mainForm.A[k] + "\r\n");
                    }
                }
                else if(receive_length == 11)
                {
                    byte[] CRC_byte = CRC16(receive_data, 9);
                    if (CRC_byte[0] != receive_data[9] || CRC_byte[1] != receive_data[10]) return;
                    for (int k = 0; k < 3; k++)
                    {
                        mainForm.A1[k] = Acc_decode(receive_data, 3 + 2 * k);
                        //Console.Write(mainForm.A[k] + "\r\n");
                    }
                }

                mainForm._A1[0] = (float)Math.Sin(- mainForm.angle1[1] / 180f * Math.PI);
                mainForm._A1[1] = (float)Math.Cos(mainForm.angle1[1] / 180f * Math.PI) * (float)Math.Sin(mainForm.angle1[0] / 180f * Math.PI);
                mainForm._A1[2] = (float)Math.Cos(mainForm.angle1[1] / 180f * Math.PI) * (float)Math.Cos(mainForm.angle1[0] / 180f * Math.PI);
            }
            if (receive_data[0] == (byte)imu_type.WT901C_485 && receive_data[1] == 0x03) 
            {
                if (receive_length == 29)
                {
                    byte[] CRC_byte = CRC16(receive_data, 27);
                    if (CRC_byte[0] != receive_data[27] || CRC_byte[1] != receive_data[28]) return;
                    for (int k = 0; k < 3; k++)
                    {
                        mainForm.A2[k] = Acc_decode(receive_data, 3 + 2 * k);
                        mainForm.angle2[k] = angle2decode(receive_data, 21 + 2 * k);
                        //Console.Write(mainForm.angle2[k] + "\r\n");
                    }

                    mainForm._A2[0] = (float)Math.Sin(-mainForm.angle2[1] / 180f * Math.PI);
                    mainForm._A2[1] = (float)Math.Cos(mainForm.angle2[1] / 180f * Math.PI) * (float)Math.Sin(mainForm.angle2[0] / 180f * Math.PI);
                    mainForm._A2[2] = (float)Math.Cos(mainForm.angle2[1] / 180f * Math.PI) * (float)Math.Cos(mainForm.angle2[0] / 180f * Math.PI);
                }
                //if (receive_length == 11)
                //{
                //    byte[] CRC_byte = CRC16(receive_data, 9);
                //    if (CRC_byte[0] != receive_data[9] || CRC_byte[1] != receive_data[10]) return;
                //    for (int k = 0; k < 3; k++)
                //    {
                //        mainForm.A2[k] = Acc_decode(receive_data, 3 + 2 * k);
                //        //mainForm.angle2[k] = angle2decode(receive_data, 21 + 2 * k);
                //        //Console.Write(mainForm.angle2[k] + "\r\n");
                //    }
                //}
            }
            if(receive_data[0] == (byte)laser.laser1 || receive_data[0] == (byte)laser.laser2 || receive_data[0] == (byte)laser.laser3)
            {
                if (receive_length != 9) return;
                byte[] CRC_byte = CRC16(receive_data, 7);
                if (CRC_byte[0] != receive_data[7] || CRC_byte[1] != receive_data[8]) return;
                mainForm.distace[receive_data[0] - 3] = receive_data[5] << 8 | receive_data[6];
            }
        }

        public static float angle4decode(byte[] receive_data, int start_num)
        {
            return (receive_data[start_num + 2] << 24 | receive_data[start_num + 3] << 16 |
                receive_data[start_num] << 8 | receive_data[start_num + 1]) / 1000f;
        }

        public static float angle2decode(byte[] receive_data, int start_num)
        {
            return (receive_data[start_num] << 8 | receive_data[start_num + 1]) / 32768f * 180;
        }

        public static float Acc_decode(byte[] receive_data, int start_num)
        {
            int temp = receive_data[start_num] << 8 | receive_data[start_num + 1];
            if (temp < 0x8000)
                return temp / 32768f * 16;
            else
                return - (0xFFFF - temp) / 32768f * 16;
        }
    }
}
